#include "MWTest.h"

void MotorBusSend(uint8_t busId, uint8_t can_id, uint8_t *data, uint8_t dataSize) {
    if(busId == 0x001) {
        /*等待发送总线空闲*/
        while(!((hcan1.Instance->TSR & CAN_TSR_TME0) && (hcan1.Instance->TSR & CAN_TSR_TME1) && (hcan1.Instance->TSR & CAN_TSR_TME2)));
        /*发送函数*/
        CAN_Send_StdDataFrame(&hcan1, can_id, data);
    }
}

/* 用户自创建总线消息函数 */
void MotorNotice(uint8_t busId, uint8_t nodeId, MW_CMD_ID cmdId) {
    if(busId == 0x001) return;
}    

/* 电机数据接收 */
MW_MOTOR_DATA MWtestData;
/* 电机登记数据 */
MW_MOTOR_ACCESS_INFO MWtest = {.busId = 1, 
                               .nodeId = 0, 
                               .motorData = &MWtestData, 
                               .sender = MotorBusSend, 
                               .notifier = MotorNotice};
/* ENDPOINT测试数据 */
MW_ENDPOINT endPointTest = {.opcode = MW_ENDPOINT_OPCODE_WRITE, 
                            .id = 95, 
                            .type = MW_ENDPOINT_DATA_TYPE_FLOAT,
                            .data.floatData = 50.0};                           

void MWFunctionTest(void) {
    /* 电机总线创建 */
    MWRegisterMotor(MWtest);
    /* 写入线电流最大值 */
    MWRxTxSdo(1, 0, endPointTest);
    /* 电机上电校准 */
    MWSetAxisState(1, 0, MW_AXIS_STATE_MOTOR_CALIBRATION);
    HAL_Delay(100);
    while(MWtest.motorData->heartBeat.currentState == MW_AXIS_STATE_MOTOR_CALIBRATION)
        HAL_Delay(10);
    MWSetAxisState(1, 0, MW_AXIS_STATE_ENCODER_OFFSET_CALIBRATION);
    HAL_Delay(100);
    while(MWtest.motorData->heartBeat.currentState == MW_AXIS_STATE_ENCODER_OFFSET_CALIBRATION)
        HAL_Delay(10);
#if MODE_TEST == 1    
    /* 设置模式为位置滤波控制模式 */
    MWSetControllerMode(1, 0, MW_POSITION_CONTROL, MW_POSITION_FILTERING_INPUT);
    /* 设置惯量值为0 */
    MWSetTrajInertia(1, 0, 0);
    /* 进入闭环控制状态 */
    MWSetAxisState(1, 0, MW_AXIS_STATE_CLOSED_LOOP_CONTROL);
    HAL_Delay(10);
    /* 输入控制位置 */
    MWPosControl(1, 0, 10, 0, 0);
#elif MODE_TEST == 2    
    /* 设置模式为斜坡速度控制模式 */
    MWSetControllerMode(1, 0, MW_VELOCITY_CONTROL, MW_RAMP_RATE_INPUT);
    /* 进入闭环控制状态 */
    MWSetAxisState(1, 0, MW_AXIS_STATE_CLOSED_LOOP_CONTROL);
    /* 输入控制速度 */
    MWVelControl(1, 0, 10, 0);
#endif
}

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan){
    /* 判断是否是CAN1收到的数据 */
    if(hcan->Instance == CAN1) {
        uint32_t canId = CAN_Receive_DataFrame(&hcan1, CAN1_buff);
        /* 传入CAN接收的数据和ID */
        MWReceiver(1, canId, CAN1_buff);
    }
}
